#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <math.h>

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    QList<QNetworkInterface> ifaces = QNetworkInterface::allInterfaces();
    for(const QNetworkInterface &iface: qAsConst(ifaces))
    {
        ui->cb_interface->addItem(iface.name());
    }

    mUdpRecAxisP = new UdpReceive(this);
    mUdpRecAxisV = new UdpReceive(this);
    mUdpRecAxisR = new UdpReceive(this);
    mUdpRecSlow = new UdpReceive(this);
    mUdpRecRepo = new UdpReceive(this);

    mRosPubData.p = 0;
    mRosPubData.v = 0;
    mRosPubData.r = 0;

    // P
    connect(mUdpRecAxisP, &UdpReceive::SnSendAngleResult, this, &MainWindow::SlHandleAngle);
    connect(mUdpRecAxisP, &UdpReceive::SnConnectSuccess, this, [this]()
    {
        ui->btnConnect->setText(tr("断开"));
        ui->p1_cb_p->setChecked(true);
    });
    connect(mUdpRecAxisP, &UdpReceive::SnDisconnect, this, [this]()
    {
        ui->btnConnect->setText(tr("连接"));
        ui->p1_cb_p->setChecked(false);
    });

    // V
    connect(mUdpRecAxisV, &UdpReceive::SnSendAngleResult, this, [this](S_AngleType t)
    {
        ui->p2_ln_initCode->display(t.initCode);
        ui->p2_ln_addupCode->display(t.addupCode);
        ui->p2_ln_as5600->display(t.asCurrCode);
        ui->p2_ln_angleInit->display(t.angleInit);
        ui->p2_ln_currCode->display(t.angleCurr);
        ui->p2_ln_step->display(t.step);
        ui->p2_ln_time->display((int)t.time);
    });
    connect(mUdpRecAxisV, &UdpReceive::SnConnectSuccess, this, [this]()
    {
        ui->btnConnect->setText(tr("断开"));
        ui->p1_cb_v->setChecked(true);
    });
    connect(mUdpRecAxisV, &UdpReceive::SnDisconnect, this, [this]()
    {
        ui->btnConnect->setText(tr("连接"));
        ui->p1_cb_v->setChecked(false);
    });

    // R
    connect(mUdpRecAxisR, &UdpReceive::SnSendAngleResult, this, [this](S_AngleType t)
    {
        ui->p3_ln_initCode->display(t.initCode);
        ui->p3_ln_addupCode->display(t.addupCode);
        ui->p3_ln_as5600->display(t.asCurrCode);
        ui->p3_ln_angleInit->display(t.angleInit);
        ui->p3_ln_currCode->display(t.angleCurr);
        ui->p3_ln_step->display(t.step);
        ui->p3_ln_time->display((int)t.time);

        // mRosPubData.r = -t.angleCurr * M_PI / 180;
        // mRosNodeHandle.SnNodePublic('r', mRosPubData);
    });
    connect(mUdpRecAxisR, &UdpReceive::SnConnectSuccess, this, [this]()
    {
        ui->btnConnect->setText(tr("断开"));
        ui->p1_cb_r->setChecked(true);
    });
    connect(mUdpRecAxisR, &UdpReceive::SnDisconnect, this, [this]()
    {
        ui->btnConnect->setText(tr("连接"));
        ui->p1_cb_r->setChecked(false);
    });

    // SLOW
    connect(mUdpRecSlow, &UdpReceive::SnSendSlowAngleResult, this, [this](S_AngleSlowType t)
    {
        double tap = atan2f(t.py, t.px);
        double tav = atan2f(t.vy, t.vx);
        double tar = atan2f(t.ry, t.rx);
        double tp = atan2f(t.py, t.px)*180/M_PI;
        double tv = atan2f(t.vy, t.vx)*180/M_PI;
        double tr = atan2f(t.ry, t.rx)*180/M_PI;
        // mRosPubData.p = tap-85*M_PI/180;
        // mRosPubData.v = (-tav+mRosPubData.p)-35*M_PI/180;
        // mRosNodeHandle.SnNodePublic('r', mRosPubData);

// qDebug() << mRosPubData.v;
        // qDebug() << t.px << t.py << t.vx << t.vy
        //          << tp
        //          << tv
        //          << tr;
        ui->p5_ln_anglep->display(tp);
        ui->p5_ln_anglev->display(tv);
        ui->p5_ln_angler->display(tr);
    });
    connect(mUdpRecSlow, &UdpReceive::SnConnectSuccess, this, [this]()
    {
        ui->p1_cb_slow->setText(tr("断开"));
        ui->p1_cb_slow->setChecked(true);
    });
    connect(mUdpRecSlow, &UdpReceive::SnDisconnect, this, [this]()
    {
        ui->p1_cb_slow->setText(tr("连接"));
        ui->p1_cb_slow->setChecked(false);
    });

    // SLOW
    connect(mUdpRecRepo, &UdpReceive::SnSendRepoAngleResult, this,
            [this](S_AngleReportType t)
            {
                mRosPubData.p = (t.p - 85) * M_PI / 180;
                mRosPubData.v = (mRosPubData.p) - (t.v + 40) * M_PI / 180;
                mRosPubData.r = t.r * M_PI / 180;
                mRosNodeHandle.SnNodePublic('r', mRosPubData);

                ui->p6_ln_anglep->display(t.p);
                ui->p6_ln_anglev->display(t.v);
                ui->p6_ln_angler->display(t.r);
            });
    connect(mUdpRecRepo, &UdpReceive::SnConnectSuccess, this, [this]()
    {
        ui->p1_cb_slow->setText(tr("断开"));
        ui->p1_cb_slow->setChecked(true);
    });
    connect(mUdpRecRepo, &UdpReceive::SnDisconnect, this, [this]()
    {
        ui->p1_cb_slow->setText(tr("连接"));
        ui->p1_cb_slow->setChecked(false);
    });

}

MainWindow::~MainWindow()
{
    delete ui;
}


void MainWindow::on_btnConnect_clicked()
{
//    if(mUdpRecAxisX->GetConnectStatus())
    mUdpRecAxisP->SnRequestConnect(QHostAddress("0.0.0.0"), 3355,
                                   QHostAddress("231.10.11.12"),
                                   QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()),
                                   ui->cb_isUseInterface->isChecked());
    mUdpRecAxisV->SnRequestConnect(QHostAddress("0.0.0.0"), 3365,
                                   QHostAddress("231.10.11.12"),
                                   QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()),
                                   ui->cb_isUseInterface->isChecked());
    mUdpRecAxisR->SnRequestConnect(QHostAddress("0.0.0.0"), 3375,
                                   QHostAddress("231.10.11.12"),
                                   QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()),
                                   ui->cb_isUseInterface->isChecked());
    mUdpRecSlow->SnRequestConnect(QHostAddress("0.0.0.0"), 3455,
                                  QHostAddress("231.10.11.12"),
                                  QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()),
                                  ui->cb_isUseInterface->isChecked());
    mUdpRecRepo->SnRequestConnect(QHostAddress("0.0.0.0"), 3511,
                                  QHostAddress("231.10.11.12"),
                                  QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()),
                                  ui->cb_isUseInterface->isChecked());
}

void MainWindow::SlHandleAngle(S_AngleType t)
{
    ui->ln_initCode->display(t.initCode);
    ui->ln_addupCode->display(t.addupCode);
    ui->ln_as5600->display(t.asCurrCode);
    ui->ln_angleInit->display(t.angleInit);
    ui->ln_currCode->display(t.angleCurr);
    ui->ln_step->display(t.step);
    ui->ln_time->display((int)t.time);
}

void MainWindow::on_btn_send_clicked()
{

    QUdpSocket *mUdpSocketAll=new QUdpSocket();

    mUdpSocketAll->bind(QHostAddress("10.42.0.1"), 3523,
                        QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
    mUdpSocketAll->joinMulticastGroup(QHostAddress("231.10.11.12"),
                                      QNetworkInterface::allInterfaces().at(ui->cb_interface->currentIndex()));

    qDebug() << "one";
    S_COMMAND_TYPE data;
    memcpy(data.head, "TMS2", 4);
    data.l1 = ui->m_dsb_p->value();
    data.l2 = ui->m_dsb_v->value();
    data.angler = ui->m_dsb_r->value();
    data.typelocate = 0;
    data.anglespeed = ui->m_dsb_speed->value();
    QByteArray dataArray((char *)&data, sizeof(S_COMMAND_TYPE));
    mUdpSocketAll->writeDatagram(dataArray, QHostAddress("231.10.11.12"), 3522);
    QTimer::singleShot(5000, this, [mUdpSocketAll]()
                       { mUdpSocketAll->deleteLater(); });
}
